package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.IterativeRobot;


public class Spike extends IterativeRobot {
    
    BallCollector ballcollector = new BallCollector();
    Collar collar = new Collar();
    CowCatcher cowcatcher =  new CowCatcher();
    DriveTrain drivetrain = new DriveTrain();
    Shooter shooter = new Shooter();
    Turret turret = new Turret();
    OI OI = new OI();

    public void robotInit() {
        collar.collarInit();
        collar.setPosition(Collar.collarStartPosition);
    }

    
    public void AutonomousContinuous() {
        ballcollector.runConveyor();
        while(ballcollector.getBallCount()>0){
            shooter.firePuncher();
        }
    }

    public void teleopContinuous() {
        
        if(OI.collectorOn.get() == true){
            ballcollector.manualRunConveyor();
        }
        else if(OI.collectorOff.get() == true){
            ballcollector.stopConveyor();
        }
        else{
            ballcollector.runConveyor();
        }
    }
    
    public void teleopPeriodic() {
         //DriveTrain
        drivetrain.tankDrive(
                drivetrain.modifier(OI.leftJoystick.getY()),
                drivetrain.modifier(OI.rightJoystick.getY()));
        
        
        //gamepad buttons:
        
        //Collar
        if (OI.button1.get() == true) {
            collar.setPosition(Collar.collarFender);
        }
        else if (OI.button2.get() == true) {
            collar.setPosition(Collar.collarLow);
        }
        else if (OI.button3.get() == true) {
            collar.setPosition(Collar.collarHigh);
        }
        else if (OI.button4.get() == true) {
            collar.setPosition(Collar.collarKey);
        }
        
        //Puncher
        else if (OI.puncherButton.get()== true) {
            shooter.firePuncher();
        }
        
        //Turret
        if (turret.turretRevLimSensor.get() == true) {
            if(OI.gamepad.getRawAxis(1)>0) {
                turret.driveMotor(0.30 * OI.gamepad.getRawAxis(1));
            }
	   else {
                turret.driveMotor(0.0);
            }
        }
        else if (turret.turretFwdLimSensor.get() == true) {
           if(OI.gamepad.getRawAxis(1)<0) {
                turret.driveMotor(0.30 * OI.gamepad.getRawAxis(1));
            }
	   else {
                turret.driveMotor(0.0);
            }
        }
    }
    
}
